#include <math.h>
#include "mmath.h"

double*  v3a;
double*  v3b;
double*  v3r;
double*  v4a;
double*  v4b;
double*  v4r;
double** m3a;
double** m3b;
double** m3r;
double** m4a;
double** m4b;
double** m4r;

char isapprox(double d1, double d2) {
	if (abs(d1 - d2) > 0.00001) printf("\tfailed: %lf != %lf\n", d1, d2);
	else printf("\tpassed\n");
}

void test_vcopy3() {
	printf("vcopy3 -------------\n");
	_vcopy3(v3a, v3r);
	isapprox(v3r[0], -2);
	isapprox(v3r[1],  8);
	isapprox(v3r[2],  3);
}

void test_vcopy4() {
	printf("vcopy4 -------------\n");
	_vcopy4(v4a, v4r);
	isapprox(v4r[0],  5);
	isapprox(v4r[1], -5);
	isapprox(v4r[2], 12);
	isapprox(v4r[3], -1);
}

void test_mmult3() {
	printf("mmult3 -------------\n");
	_mmult3(m3r, m3a, m3b);
	isapprox(m3r[0][0], 53); isapprox(m3r[0][1],  12); isapprox(m3r[0][2],  -1);
	isapprox(m3r[1][0], 22); isapprox(m3r[1][1], -44); isapprox(m3r[1][2],  78);
	isapprox(m3r[2][0], 68); isapprox(m3r[2][1],  46); isapprox(m3r[2][2], -59);
}

void test_mmult4() {
	printf("mmult4 -------------\n");
	_mmult4(m4r, m4a, m4b);
	isapprox(m4r[0][0], -86); isapprox(m4r[0][1],  76); isapprox(m4r[0][2],  86); isapprox(m4r[0][3],  58);
	isapprox(m4r[1][0], -38); isapprox(m4r[1][1], -84); isapprox(m4r[1][2], -98); isapprox(m4r[1][3],   0);
	isapprox(m4r[2][0], -24); isapprox(m4r[2][1],  56); isapprox(m4r[2][2],  38); isapprox(m4r[2][3], -12);
	isapprox(m4r[3][0],   9); isapprox(m4r[3][1], -19); isapprox(m4r[3][2], -10); isapprox(m4r[3][3],  -1);
}

void test_mvmult3() {
	printf("mvmult3 ------------\n");
	_mvmult3(v3r, m3a, v3a);
	isapprox(v3r[0], -41);
	isapprox(v3r[1],  14);
	isapprox(v3r[2], -80);
}

void test_mvmult4() {
	printf("mvmult4 ------------\n");
	_mvmult4(v4r, m4a, v4a);
	isapprox(v4r[0], 88);
	isapprox(v4r[1], 61);
	isapprox(v4r[2],  4);
	isapprox(v4r[3],  9);
}

void test_vadd3() {
	printf("vadd3 --------------\n");
	_vadd3(v3r, v3a, v3b);
	isapprox(v3r[0],  4);
	isapprox(v3r[1],  9);
	isapprox(v3r[2], 10);
}

void test_vadd4() {
	printf("vadd4 --------------\n");
	_vadd4(v4r, v4a, v4b);
	isapprox(v4r[0],   7);
	isapprox(v4r[1], -10);
	isapprox(v4r[2],  20);
	isapprox(v4r[3],   7);
}

void test_vscale3() {
	printf("vscale3 ------------\n");
	_vscale3(v3a, 3.5);
	isapprox(v3a[0], -7);
	isapprox(v3a[1], 28);
	isapprox(v3a[2], 10.5);
}

void test_vscale4() {
	printf("vscale4 ------------\n");
	_vscale4(v4a, -2.6);
	isapprox(v4a[0], -13);
	isapprox(v4a[1],  13);
	isapprox(v4a[2], -31.2);
	isapprox(v4a[3],   2.6);
}

int main() {
	//initialize vectors and matrices
	v3a = dvecalloc(3);
	v3a[0] = -2; v3a[1] = 8; v3a[2] = 3;
	v3b = dvecalloc(3);
	v3b[0] = 6; v3b[1] = 1; v3b[2] = 7;
	v3r = dvecalloc(3);
	v4a = dvecalloc(4);
	v4a[0] = 5; v4a[1] = -5; v4a[2] = 12; v4a[3] = -1;
	v4b = dvecalloc(4);
	v4b[0] = 2; v4b[1] = -5; v4b[2] = 8; v4b[3] = 8;
	v4r = dvecalloc(4);
	m3a = dmatalloc(3, 3);
	m3a[0][0] = -2; m3a[0][1] =  -3; m3a[0][2] = -7;
	m3a[1][0] =  8; m3a[1][1] =   6; m3a[1][2] = -6;
	m3a[2][0] = -9; m3a[2][1] = -10; m3a[2][2] = -6;
	m3b = dmatalloc(3, 3);
	m3b[0][0] =  2; m3b[0][1] = -10; m3b[0][2] =  9;
	m3b[1][0] = -5; m3b[1][1] =   5; m3b[1][2] = -1;
	m3b[2][0] = -6; m3b[2][1] =  -1; m3b[2][2] = -2;
	m3r = dmatalloc(3, 3);
	m4a = dmatalloc(4, 4);
	m4a[0][0] =  7; m4a[0][1] =   7; m4a[0][2] =  7; m4a[0][3] = -4;
	m4a[1][0] =  1; m4a[1][1] = -10; m4a[1][2] =  0; m4a[1][3] = -6;
	m4a[2][0] = -6; m4a[2][1] =   4; m4a[2][2] =  4; m4a[2][3] = -6;
	m4a[3][0] =  3; m4a[3][1] =  -2; m4a[3][2] = -1; m4a[3][3] =  4;
	m4b = dmatalloc(4, 4);
	m4b[0][0] = -4; m4b[0][1] = -2; m4b[0][2] = 0; m4b[0][3] =  6;
	m4b[1][0] =  1; m4b[1][1] =  7; m4b[1][2] = 8; m4b[1][3] =  3;
	m4b[2][0] = -7; m4b[2][1] =  7; m4b[2][2] = 6; m4b[2][3] = -3;
	m4b[3][0] =  4; m4b[3][1] =  2; m4b[3][2] = 3; m4b[3][3] = -4;
	m4r = dmatalloc(4, 4);
	
	//test functions
	test_vcopy3();
	test_vcopy4();
	test_mmult3();
	test_mmult4();
	test_mvmult3();
	test_mvmult4();
	test_vadd3();
	test_vadd4();
	test_vscale3();
	test_vscale4();
	return 0;
}
